package robocup.component.situations;

import java.util.Arrays;

import robocup.component.ControllerNumbers;
import robocup.component.DAInamiteConstants;
import robocup.component.PConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.INTERCEPTMODE;
import robocup.component.actions.DashAction;
import robocup.component.actions.TurnAction;
import robocup.component.geometry.Vektor;
import robocup.component.worldobjects.Player;

/**
 * This class stores precalculated data about player movements and some methods
 * which refer to this data. The data stored is the max. possible player
 * movement for a given player, a given time and a given delay (i.e. for turning
 * first). The movement's data is updated each time the player's
 * effort/stamina/recovery changes by much.<br>
 * <br>
 * FIXME
 *  - when a player already has a certain initial speed in the same direction,
 * adding his speed to the value stored here might result in a velocity higher
 * than {@link PConf#PLAYER_SPEED_MAX} leading to wrong predictions.
 */
public class Movements {

  private static final double STAMINA_THRES = 2000;

  private final WorldModel          world;

  private final Player[]    teammates;

  private final Player[]    opponents;

  // the max horizont for precalculation of the movements
  private final int         maxCycles       = DAInamiteConstants.PROPHET_MAX_CYCLES_INTERCEPTION;

  private final double[]    lastStamina     = new double[22];

  // dash100 action
  private final DashAction  dash100         = new DashAction(100);

  // dash100 backward action
  private final DashAction  dash100BW       = new DashAction(-100);

  // 179 degree for security, don't set it to 180 degree without changing the
  // methods (otherwise you will see strange things happening ;-)
  private final TurnAction  turn180         = new TurnAction(179);

  // init players so we don't have to clone later
  private final Player              p               = Player.getDummyInstance();

  /**
   * the max. possible player movements (distance to init. pos.) depending on
   * the players first actions.<br>
   * 
   * The Index:
   * 
   * <ol start=0>
   * <li>playerIndex - [0..10] teammates, [11..21] opponents</li>
   * <li>cylce</li>
   * <li>way of interception
   * <ol start=0>
   * <li>forward dashing</li>
   * <li>turning 1x</li>
   * <li>turning 2x</li>
   * <li>turning 3x</li>
   * <li>backward dashing (direct)</li>
   * <li>backward dashing after 1x turning</li>
   * </ol>
   * </li>
   * <ol>
   */
  private final double[][][] playerMovements = new double[22][this.maxCycles][6];

  /**
   * A class that stores the movements possibilities of all players depending on
   * their type (heterogeneous players) and stamina
   * 
   * @param world -
   *          the current world-model
   */
  public Movements(
      final WorldModel world) {

    this.world = world;
    this.teammates = world.getTeammates();
    this.opponents = world.getOpponents();
    this.initPlayerMovements();
  }

  /**
   * init the player movements.
   * 
   */
  private void initPlayerMovements() {

    Arrays.fill(this.lastStamina, 8000.0);
    for (final Player p : this.teammates) {
      this.setPlayerMovements(p, true);
    }
    for (final Player p : this.opponents) {
      this.setPlayerMovements(p, true);
    }
  }

  /**
   * sets the players movement (distance from initial position) for the cases:
   * 
   * <ol start=0>
   * <li>forward dashing</li>
   * <li>turning 1x</li>
   * <li>turning 2x</li>
   * <li>turning 3x</li>
   * <li>backward dashing (direct)</li>
   * <li>backward dashing after 1x turning</li>
   * </ol>
   * It is assumed that the initial player's speed is (0,0). The player's drift
   * has to be calculated seperatly! <br>
   * <br>
   * <b>Note:</b> a player does not always cover the same distance when dashing
   * or turning first and dashing than as the turning might have a positive
   * effect on stamina!! As backward dashing consumes twice as much stamina it
   * is considered seperatly as well.
   * 
   * @param player -
   *          the player
   * @param forceUpdate -
   *          if true the matrix is recalculated. This is only needed when when
   *          the player's stamina/effort changes much.
   */
  public final void setPlayerMovements(final Player player,
      boolean forceUpdate) {

    final int playerIndex = player.isFriend() ? player.getNumber() - 1 : 11 + player
        .getNumber() - 1;

    // no update wanted or needed
    if (!forceUpdate) {
    	if (player.getStamina() > STAMINA_THRES && 
    			this.lastStamina[playerIndex] > STAMINA_THRES)
    		return;
    	if (Math.abs(player.getStamina() - this.lastStamina[playerIndex]) < 
    		this.lastStamina[playerIndex] / 10.0)
    		return;
    }

    this.lastStamina[playerIndex] = player.getStamina();

    // only dashing
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][0] = this.p.getDistance(player);
      this.p.predictThisAfterAction(this.dash100);
    }

    // turning 1x
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][1] = this.p.getDistance(player);
      if (i == 0) {
        this.p.predictThisAfterAction(this.turn180);
      }
      else {
        this.p.predictThisAfterAction(this.dash100);
      }
    }

    // turning 2x
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][2] = this.p.getDistance(player);
      if (i < 2) {
        this.p.predictThisAfterAction(this.turn180);
      }
      else {
        this.p.predictThisAfterAction(this.dash100);
      }
    }

    // turning 3x
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][3] = this.p.getDistance(player);
      if (i < 3) {
        this.p.predictThisAfterAction(this.turn180);
      }
      else {
        this.p.predictThisAfterAction(this.dash100);
      }
    }

    // BackWard
    // only dashing
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i <= DAInamiteConstants.PROPHET_MAX_BACKWARD && i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][4] = this.p.getDistance(player);
      this.p.predictThisAfterAction(this.dash100BW);
    }

    // turning 1x
    this.p.copy(player);
    this.p.getSpeed().reset();

    for (int i = 0; i <= DAInamiteConstants.PROPHET_MAX_BACKWARD && i < this.maxCycles; i++) {
      this.playerMovements[playerIndex][i][5] = this.p.getDistance(player);
      if (i == 0) {
        this.p.predictThisAfterAction(this.turn180);
      }
      else {
        this.p.predictThisAfterAction(this.dash100BW);
      }
    }
  }

  /**
   * this method tells if a player can reach a given position in a given time.
   * this is an optimized Version for passing (@see PassSituation).
   * 
   * @param playerIndex -
   *          0..10 for team, 11..21 for others
   * @param cycles -
   *          max_cylces to reach position
   * @param target -
   *          target (ball pos etc.) after max_cycles
   * @param playerPosition -
   *          player pos (+ drift after 'cycles' (instead this drift might be
   *          sub. from the target!))
   * @param bodyDir -
   *          players body dir
   * @param maxTurnAngle1 -
   *          the maximum turn angle turning once
   * @param maxTurnAngle2 -
   *          the maximum turn angle turning twice
   * @param secureDistance -
   *          distance ball-player take is taken as secure
   * @return the {@link INTERCEPTMODE}
   */
  public final ControllerNumbers.INTERCEPTMODE reaches(final int playerIndex,
      final int cycles,
      final Vektor target,
      final Vektor playerPosition,
      final double bodyDir,
      final double maxTurnAngle1,
      final double maxTurnAngle2,
      final double secureDistance) {

    final double distance = target.getDistance(playerPosition);

    // point is much too far away, such that max-speed wouldn't
    // reach it!
    final double spdMax = this.world.getSelfRef().getPConf().PLAYER_SPEED_MAX;
    if ((distance - secureDistance) / spdMax > cycles) {
      return INTERCEPTMODE.NO_INTERCEPT;
    }
    else if (distance < this.playerMovements[playerIndex][cycles][3] + secureDistance) {
      // player gets ball even when turning 3x (slowest poss.)
      return ControllerNumbers.INTERCEPTMODE.TURNING3;
    }
    else {
      double angle;
      // dashing
      if (distance < this.playerMovements[playerIndex][cycles][0] + secureDistance) {
        angle = Math.atan2(target.x - playerPosition.x,
            target.y - playerPosition.y) * (180.0 / Math.PI);
        angle = Math.abs(Vektor.normalize(bodyDir - angle));
        if (Math.abs(Math.sin(Math.toRadians(angle))) * distance < secureDistance) {
          if (angle > 90.0 && cycles <= DAInamiteConstants.PROPHET_MAX_BACKWARD) {
            return ControllerNumbers.INTERCEPTMODE.DASH_BW;
          }
          if (angle <= 90.0) {
            return ControllerNumbers.INTERCEPTMODE.DASH_FW;
          }
        }
      }
      else {
        return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
      }

      // turning 1x
      if (distance < this.playerMovements[playerIndex][cycles][1] + secureDistance) {
        if (angle < maxTurnAngle1) {
          return ControllerNumbers.INTERCEPTMODE.TURNING1_FW;
        }
        // backward
        if (180.0 - angle < maxTurnAngle1 && cycles <= DAInamiteConstants.PROPHET_MAX_BACKWARD) {
          return ControllerNumbers.INTERCEPTMODE.TURNING1_BW;
        }
      }
      else {
        return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
      }

      // turning 2x
      if (distance < this.playerMovements[playerIndex][cycles][2] + secureDistance) {
        if (angle < maxTurnAngle2) {
          return ControllerNumbers.INTERCEPTMODE.TURNING2;
        }
      }
    }
    return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
  }

  /**
   * this method tells if a player can reach a given position in a given time.
   * this is an optimized Version for passing (@see PassSituation).
   * 
   * @param playerIndex -
   *          0..10 for team, 11..21 for others
   * @param cycles -
   *          max_cylces to reach position
   * @param target -
   *          target (ball pos etc.) to reach after <code>cycles</code>
   * @param playerPosition -
   *          player pos (+ drift after <code>cycles</code> (instead this
   *          drift might be sub. from the target!))
   * @param bodyDir -
   *          players body dir
   * @param maxTurnAngle1 -
   *          the maximum turn angle turning once
   * @param maxTurnAngle2 -
   *          the maximum turn angle turning twice
   * @param secureDistance -
   *          distance ball-player that is taken as secure (i.e. kick-distance)
   * @param tolerance -
   *          tolerance in % (i.e. 0.95 meaning that the player reaches an average
   *          of 95% of the max. possible velocity when going to the point,
   *          loosing 5% due to turning)
   * @return the distance left when the player tries to reach the given
   *         position. distance > 0 means the point will not be reached
   */
  public final double getLeftDistance(final int playerIndex,
      final int cycles,
      final Vektor target,
      final Vektor playerPosition,
      final double bodyDir,
      final double maxTurnAngle1,
      final double maxTurnAngle2,
      final double secureDistance,
      final double tolerance) {

    final double distance = target.getDistance(playerPosition);
    double angle = Math.atan2(target.x - playerPosition.x,
        target.y - playerPosition.y) * (180.0 / Math.PI);
    angle = Math.abs(Vektor.normalize(angle - bodyDir));

    // dash
    if (Math.abs(Math.sin(Math.toRadians(angle))) * distance < secureDistance) {
      return distance - this.playerMovements[playerIndex][cycles][0] * tolerance - secureDistance;
    }
    // 1x turning
    else if (angle < maxTurnAngle1) {
      return distance - this.playerMovements[playerIndex][cycles][1] * tolerance - secureDistance;
    }
    // 2x turning
    else if (angle < maxTurnAngle2) {
      return distance - this.playerMovements[playerIndex][cycles][2] * tolerance - secureDistance;
    }
    // 3x turning
    else {
      return distance - this.playerMovements[playerIndex][cycles][3] * tolerance - secureDistance;
    }
  }

  
  /**
   * The playerMovements.
   *
   * @return the playerMovements
   */
  public final double[][][] getPlayerMovements() {
  
    return this.playerMovements;
  }
}
